function e=dist(T1,T2)
e=sqrt((T1(1,4)-T2(1,4)).^2+(T1(2,4)-T2(2,4)).^2+(T1(3,4)-T2(3,4)).^2);